#include "bl602StateHandle.h"
#include "protocol.h"
#include "vi5300.h"
#include "uart.h"
#include <math.h>

static uint8_t targetAccTime = 3;
static uint8_t targetAltTime = 1;
static uint16_t heightMax = 80;
static uint16_t heightMin = 40;
static uint16_t height = 0;
static int16_t accMin = -140;
static int16_t accMax = -50;
static int16_t accAvg = -95;
static int32_t accSum = 0;
static uint8_t updateAvgFlag = 1;

void resetAvg(void)
{
    updateAvgFlag = 1;
}

void setHeight(uint16_t min, uint16_t max)
{
    heightMax = max;
    heightMin = min;
}

uint16_t getHeight(void)
{
    return height;
}

void setAcc(int16_t min, int16_t max)
{
    accMax = max;
    accMin = min;
}

void setTime(uint8_t time)
{
    targetAccTime = time;
}

void setAccAvg(int16_t avg)
{
    accAvg = avg;
}

int16_t getAccAvg(void)
{
    return accAvg;
}

void accDataHandle(int16_t acc, float dt)
{
    static float dtSum;
    static float currTime;
    static float currAltTime;
    static uint16_t heightBuff[5];
    static uint8_t heightIndex;
    // 初始值给一个比acc都小/大的值
    static int16_t currAccMax = -30000;
    static int16_t currAccMin = 30000;

    dtSum += dt;
    currTime += dt;
    currAltTime += dt;

    if (updateAvgFlag)
    {
        static uint8_t count;
        count++;
        accSum += acc;
        if (count > 49)
        {
            count = 0;
            updateAvgFlag = 0;
            accAvg = accSum * 0.001;
            accSum = 0;
        }
    }

    if (acc > currAccMax)
    {
        currAccMax = acc;
    }
    if (acc < currAccMin)
    {
        currAccMin = acc;
    }

    if (dtSum > 0.05f)
    {
        dtSum = 0;
#if 1
        static uint8_t heightType = 0;
        float t_height = 0;
        VI5300_Get_Measure_Data(&t_height, 0);
        heightBuff[heightIndex] = t_height;
        heightIndex++;
        if (heightIndex == 5)
        {
            heightIndex = 0;
            uint16_t sum = (heightBuff[0] + heightBuff[2] + heightBuff[3] + heightBuff[4] + heightBuff[1]) * 0.2;
            if (fabs(heightBuff[4] - sum) < 15)
            {
                height = heightBuff[4];
            }
        }

        if (height > heightMax)
        {
            if (heightType != 1 && currAltTime > targetAltTime)
            {
                heightType = 1;
                uint8_t t_data = 2;
                receiveData(1, 1, &t_data);
                currAltTime = 0;
            }
        }
        else if (height < heightMin)
        {
            if (heightType != 2 && currAltTime > targetAltTime)
            {
                heightType = 2;
                uint8_t t_data = 1;
                receiveData(1, 1, &t_data);
                currAltTime = 0;
            }
        }
        else
        {
            if (heightType != 3 && currAltTime > targetAltTime)
            {
                heightType = 3;
                uint8_t t_data = 3;
                receiveData(1, 1, &t_data);
                currAltTime = 0;
            }
        }

        int16_t delta = (currAccMax - currAccMin) * 0.05;
        if (delta > 75)
        {
            if (currTime > targetAccTime && (delta > 300 || heightType == 3))
            {
                uint8_t t_data = 2;
                receiveData(2, 1, &t_data);
                currTime = 0;
                currAltTime = 0;
                heightType = 0;
            }
        }
        // else
        // {
        //     if (currAccMax*0.05 > accMax)
        //     {
        //         if (currTime > targetAccTime && heightType == 3)
        //         {
        //             uint8_t t_data = 2;
        //             receiveData(2, 1, &t_data);
        //             currTime = 0;
        //             currAltTime = 0;
        //             heightType = 0;
        //         }
        //     }
        //     else if (currAccMin*0.05 < accMin)
        //     {
        //         if (currTime > targetAccTime && heightType == 3)
        //         {
        //             uint8_t t_data = 1;
        //             receiveData(2, 1, &t_data);
        //             currTime = 0;
        //             currAltTime = 0;
        //             heightType = 0;
        //         }
        //     }
        // }

        currAccMax = -30000;
        currAccMin = 30000;
#else
#define BYTE0(dwTemp) (*((uint8_t *)(&dwTemp)))
#define BYTE1(dwTemp) (*((uint8_t *)(&dwTemp) + 1))
        uint8_t _cnt = 4;
        int16_t _temp;
        int16_t a_x = currAccMax * 0.05;
        int16_t a_y = currAccMin * 0.05;
        int16_t a_z = acc * 0.05;
        int16_t g_x = 0;
        int16_t g_y = 0;
        int16_t g_z = 0;
        int16_t m_x = 0;
        int16_t m_y = 0;
        int16_t m_z = 0;
        uint8_t sendData[64] = {0xAA, 0xAA, 0x02, 20};
        // 取均值作为每次的最大最小值
        currAccMax = -30000;
        currAccMin = 30000;

        _temp = a_x;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        _temp = a_y;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        _temp = a_z;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);

        _temp = g_x;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        _temp = g_y;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        _temp = g_z;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);

        _temp = m_x;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        _temp = m_y;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        _temp = m_z;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        _temp = 0;
        sendData[_cnt++] = BYTE1(_temp);
        sendData[_cnt++] = BYTE0(_temp);
        uint8_t cksum = 0;
        for (int i = 0; i < sendData[3] + 4; i++)
        {
            cksum += sendData[i];
        }
        sendData[sendData[3] + 4] = cksum;
        uartSendData(sendData, sendData[3] + 5);
#endif
    }
}
